//
// Created by Raymoond Tang on 19/4/13.
// Copyright (c) 2013 NUS. All rights reserved.
//
// To change the template use AppCode | Preferences | File Templates.
//


#include <boost/concept_check.hpp>
#include "VelocityTuning.h"
#include "math.h"

static double MaxTime = 100.0;

#ifndef DBL_EPSILON
#define DBL_EPSILON     2.2204460492503131e-016
#endif

VelocityTuning::~VelocityTuning() {
}

void VelocityTuning::updateState(Robot2D robot, Segment2D path, const std::vector<PlayerObject>& obstacles) {

    list< pair<double , double > > range_set;

    cout << "Path:" << path.toString() <<endl;

    for (std::string::size_type i = 0; i < obstacles.size(); ++i) {
        if (obstacles[i].VelX == 0 && obstacles[i].VelY == 0) {
//            continue;
        }
//        cout << "Obstacle: " << i << endl;
//        cout << "High x:\t"<<obstacles[i].PosHighX << endl;
//        cout << "Low x:\t"<<obstacles[i].PosLowX << endl;
//        cout << "High y:\t"<<obstacles[i].PosHighY << endl;
//        cout << "Low y:\t"<<obstacles[i].PosLowY << endl;
//        cout << "Vel x:\t"<<obstacles[i].VelX << endl;
//        cout << "Vel Y:\t"<<obstacles[i].VelY << endl;
//        cout << "Vel Yaw:\t"<<obstacles[i].VelYaw << endl;
        boost::shared_ptr<CollisionRange> r(new CollisionRange(path,obstacles[i],MaxTime));

        std::pair<double, double> range = r->getRange();
        joinRange(range_set, range);
//        cout << range.first <<"," << range.second << endl;
    }
    pair<double,double> top (MaxTime,MaxTime);
    range_set.push_back(top);
    for (list<pair<double, double> >::iterator it = range_set.begin(); it != range_set.end(); ++it) {
//        cout << it->first <<"," << it->second << endl;
    }
   

    double length = path.getLength();
    double time = length / MaxVelocity;
    sol = locateSolRange(range_set,time);
    goal = path.end;
//    cout << path.toString() << "Length: " << length << endl;
    cout << "Time: " << time << " Sol: "<<sol.first << "," << sol.second <<endl;

    if (fabs(sol.first - 0) < DBL_EPSILON && fabs(sol.second - 0) < DBL_EPSILON) {
        velocity.x = 0.0;
        velocity.y = 0.0;
    } else {
        velocity.x = MaxVelocity*(path.end.x - path.start.x)/length;
        velocity.y = MaxVelocity*(path.end.y - path.start.y)/length;
    }
}

Robot2D VelocityTuning::getVelocity(double time) {
    return velocity;
//    cout << "current time: " << time << endl;
//    if(time >= sol.first && time < sol.second) {
//        return velocity;
//    } else {
//        Robot2D zeroVel;
//        return  zeroVel;
//    }
}

Robot2D VelocityTuning::getVelocity(Segment2D path) {
    double length = path.getLength();
    velocity.x = MaxVelocity*(path.end.x - path.start.x)/length;
    velocity.y = MaxVelocity*(path.end.y - path.start.y)/length;
    return velocity;
}
void VelocityTuning::joinRange(list<pair<double, double> >  & v_set, pair<double, double> new_range) {

    list<pair<double, double> > ranges (v_set);
    ranges.push_back(new_range);
    list<pair<double, double> > result;
    ranges.sort();
    list<pair<double, double> >::iterator it = ranges.begin();
    pair<double,double> current = *(it)++;
    while (it != ranges.end()){
        if (current.second >= it->first){ // you might want to change it to >=
            current.second = std::max(current.second, it->second);
        } else {
            result.push_back(current);
            current = *(it);
        }
        it++;
    }
    result.push_back(current);
    v_set = result;
}

pair<double, double>  VelocityTuning::locateSolRange(list<pair<double, double> >  & v_set,double timeNeeded) {
    pair<double,double> sol(0.0,0.0);

    list<pair<double, double> >::iterator it = v_set.begin();
    pair<double,double> current = *(it)++;
    if (current.first > timeNeeded) {
        sol.second = timeNeeded;
        return  sol;
    }
    while (it != v_set.end()){
        if ((it->first - current.second) > timeNeeded){ // you might want to change it to >=
            sol.first = current.second;
            sol.second = current.second + timeNeeded;
            return sol;
        } else {
            current = *(it);
        }
        it++;
    }
    return sol;
}

Robot2D VelocityTuning::getVelocity(double time,const Robot2D &pos) {
    Segment2D path(pos,goal);
    double length = path.getLength();
    Robot2D curVel;
    curVel.x = MaxVelocity*(path.end.x - path.start.x)/length;
    curVel.y = MaxVelocity*(path.end.y - path.start.y)/length;
    if(time >= sol.first && time < sol.second) {
        return curVel;
    } else {
        Robot2D zeroVel;
        return  zeroVel;
    }
}

pair<double,double> VelocityTuning::getSol() {
    return sol;
}

void VelocityTuning::updateState(Robot2D robot_pos, vector<Position> path, vector<PlayerObject> obstacles) {
    list<list<pair<double, double> > > v_ranges;
    vector<pair<double, double> > time_range;
    for(vector<Position>::iterator it = path.begin(); it!= path.end() - 1; ++it) {
        list<pair<double, double> > o_range;
        for(vector<PlayerObject>::iterator jt = obstacles.begin(); jt!= obstacles.end(); jt++) {
            Robot2D a(it->x,it->y);
            Robot2D b((it+1)->x,(it+1)->y);
            Segment2D p(a,b);
            PlayerObject o = *jt;
            double time = p.getLength() / MaxVelocity;
            pair<double,double> p_time(0,time);
            time_range.push_back(p_time);
            boost::shared_ptr<CollisionRange> r(new CollisionRange(p,o,MaxTime));

            std::pair<double, double> range = r->getRange();
            joinRange(o_range, range);
            cout <<"Seg:" << p.toString();
            cout <<"Path:" << it - path.begin() <<" "<< range.first <<"," << range.second << endl << endl;
        }

        v_ranges.push_back(o_range);
    }
    int max_pos = 0;
    double max_back = 0;
    int i =0;

    
    for(list<list<pair<double, double> > >::iterator it = v_ranges.begin(); it != v_ranges.end(); ++it,++i) {
        list<pair<double, double> > curr_range = *it;
        if (curr_range.size() <= 0) {
            continue;
        }
        pair<double, double> back = curr_range.back();
        cout <<"back: " << back.second << endl;;
        
        if (back.second > max_back) {
            double time_lag = 0.0;
            for(int j = max_pos; j <= i; ++j) {
                time_lag += time_range[j].second;
            }
            cout <<"time_lag: " << time_lag << endl;;
            if (back.second > (max_back + time_lag)) {
                max_back = back.second;
                max_pos = i;
            }
        }
    }
    
    double wait_time = max_back;
    cout << "max_back: " << wait_time << " pos: " << max_pos << endl;
    if (wait_time == 0) {
        waitTime = wait_time;
        return;
    }
    for (int i = max_pos; i >= 0; --i) {
        wait_time -= time_range[i].second;
    }
    waitTime = wait_time;
    cout << "wait Time: " << wait_time << endl;
}



//int main() {
//    VelocityTuning velocityTuner(10);
//    list<pair<double,double> > v_set;
//    pair<double, double> r1(1.0,2.0);
//    pair<double, double> r2(3.0,7.0);
//    pair<double, double> r3(0.0,0.3);
//    pair<double, double> r4(4.0,6.5);
//    pair<double, double> r5(0.5,1.5);
//    pair<double ,double> r6(1,7);
//    pair<double,double> top (MaxTime,MaxTime);
//
//
////    v_set.push_back(r1);
////    v_set.push_back(r2);
////    v_set.push_back(r3);
//    velocityTuner.joinRange(v_set, r1);
//    velocityTuner.joinRange(v_set, r2);
//    velocityTuner.joinRange(v_set, r3);
//    velocityTuner.joinRange(v_set, r4);
//    velocityTuner.joinRange(v_set, r5);
//    velocityTuner.joinRange(v_set, r6);
//
//    v_set.push_back(top);
//
//    pair<double,double> sol = velocityTuner.locateSolRange(v_set, 0.2);
//
//    for(list<pair<double,double> >::iterator it = v_set.begin(); it != v_set.end(); ++it) {
//       cout << it->first<<","<< it->second << "|";
//    }
//    cout << endl;
//    cout << sol.first << "," << sol.second;
//    return 0;
//}